/*
 * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
 *
 * This file is part of SeMoLearning.
 *
 * SeMoLearning is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SeMoLearning is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SeMoLearning.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "semol/core/Semol.h"
#include "semol/core/Version.h"
#include "semol/core/Sensor.h"
#include "semol/core/Actuator.h"

#include "semol/core/Memory.h"
#include "semol/core/BayesFilter.h"

#include <utilite/UDirectory.h>
#include <utilite/UConversion.h>
#include <utilite/UTimer.h>
#include <utilite/UMath.h>
#include <utilite/UFile.h>

#include "SimpleIni.h"

#include <stdlib.h>
#include <set>

#define DB_TYPE "sqlite3"

#define LOG_F "LogF.txt"
#define LOG_I "LogI.txt"

#define GRAPH_FILE_NAME "Graph.dot"

namespace semol
{
const char * Semol::kDefaultIniFileName = "semol.ini";
const char * Semol::kDefaultDatabaseName = "LTM.db";

Semol::Semol() :
	_publishStats(Parameters::defaultMainPublishStats()),
	_publishRawData(Parameters::defaultMainPublishRawData()),
	_publishPdf(Parameters::defaultMainPublishPdf()),
	_publishLikelihood(Parameters::defaultMainPublishLikelihood()),
	_publishSensorData(Parameters::defaultSMPublishSensorData()),
	_maxTimeAllowed(Parameters::defaultMainTimeThr()), // 700 ms
	_maxMemoryAllowed(Parameters::defaultMainMemoryThr()), // 0=inf
	_sensorsBufferMaxSize(Parameters::defaultMainSMStateBufferSize()),
	_loopThr(Parameters::defaultMainLoopThr()),
	_loopRatio(Parameters::defaultMainLoopRatio()),
	_loopSimilarity(Parameters::defaultVhSimilarity()),
	_retrievalThr(Parameters::defaultMainRetrievalThr()),
	_maxRetrieved(Parameters::defaultMainMaxRetrieved()),
	_selectionNeighborhoodSummationUsed(Parameters::defaultMainSelectionNeighborhoodSummationUsed()),
	_selectionLikelihoodUsed(Parameters::defaultMainSelectionLikelihoodUsed()),
	_actionsSentRejectHyp(Parameters::defaultMainActionsSentRejectHyp()),
	_confidenceThr(Parameters::defaultMainConfidenceThr()),
	_likelihoodStdDevRemoved(Parameters::defaultMainLikelihoodStdDevRemoved()),
	_likelihoodNullValuesIgnored(Parameters::defaultMainLikelihoodNullValuesIgnored()),
	_neighborhoodActionsSelection(Parameters::defaultMainNeighborhoodActionsSelection()),
	_lcHypothesisId(0),
	_reactivateId(0),
	_lastLcHypothesisValue(0),
	_lastLoopClosureId(0),
	_actuatorsLink(0),
	_bayesFilter(0),
	_memory(0)
{
	ULOGGER_DEBUG("Working directory=%s", Parameters::defaultMainWorkingDirectory().c_str());
	this->setWorkingDirectory(Parameters::defaultMainWorkingDirectory());
}

Semol::~Semol() {
	UDEBUG("");

	this->close();
}

std::string Semol::getVersion()
{
	return SEMOL_VERSION;
	return ""; // Second return only to avoid compiler warning with RTABMAP_VERSION not yet set.
}

std::string Semol::getIniFilePath()
{
	std::string privatePath = UDirectory::homeDir() + "/.semolearning";
	if(!UDirectory::exists(privatePath))
	{
		UDirectory::makeDir(privatePath);
	}
	return (privatePath + "/") + kDefaultIniFileName;
}

void Semol::close()
{
	if(_memory)
	{
		delete _memory;
		_memory = 0;
	}
	if(_bayesFilter)
	{
		delete _bayesFilter;
		_bayesFilter = 0;
	}
}

void Semol::init(const ParametersMap & parameters, bool deleteMemory)
{
	if(deleteMemory)
	{
		// Set the working directory and database path before
		ParametersMap::const_iterator iter;
		if((iter=parameters.find(Parameters::kMainWorkingDirectory())) != parameters.end())
		{
			this->setWorkingDirectory(iter->second.c_str());
		}
		this->resetMemory(true);
	}

	this->parseParameters(parameters);
}

void Semol::init(const std::string & configFile, bool deleteMemory)
{
	// fill ctrl struct with values from the configuration file
	ParametersMap param;// = Parameters::defaultParameters;

	if(!configFile.empty())
	{
		ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
		this->readParameters(configFile, param);
	}

	this->init(param, deleteMemory);
}

void Semol::parseParameters(const ParametersMap & parameters)
{
	ULOGGER_DEBUG("");
	ParametersMap::const_iterator iter;
	Parameters::parse(parameters, Parameters::kMainPublishStats(), _publishStats);
	Parameters::parse(parameters, Parameters::kMainPublishRawData(), _publishRawData);
	Parameters::parse(parameters, Parameters::kMainPublishPdf(), _publishPdf);
	Parameters::parse(parameters, Parameters::kMainPublishLikelihood(), _publishLikelihood);
	Parameters::parse(parameters, Parameters::kSMPublishSensorData(), _publishSensorData);
	Parameters::parse(parameters, Parameters::kMainTimeThr(), _maxTimeAllowed);
	Parameters::parse(parameters, Parameters::kMainMemoryThr(), _maxMemoryAllowed);
	Parameters::parse(parameters, Parameters::kMainLoopThr(), _loopThr);
	Parameters::parse(parameters, Parameters::kMainLoopRatio(), _loopRatio);
	Parameters::parse(parameters, Parameters::kVhSimilarity(), _loopSimilarity);
	Parameters::parse(parameters, Parameters::kMainRetrievalThr(), _retrievalThr);
	Parameters::parse(parameters, Parameters::kMainSMStateBufferSize(), _sensorsBufferMaxSize);
	Parameters::parse(parameters, Parameters::kMainMaxRetrieved(), _maxRetrieved);
	Parameters::parse(parameters, Parameters::kMainSelectionNeighborhoodSummationUsed(), _selectionNeighborhoodSummationUsed);
	Parameters::parse(parameters, Parameters::kMainSelectionLikelihoodUsed(), _selectionLikelihoodUsed);
	Parameters::parse(parameters, Parameters::kMainActionsSentRejectHyp(), _actionsSentRejectHyp);
	Parameters::parse(parameters, Parameters::kMainConfidenceThr(), _confidenceThr);
	Parameters::parse(parameters, Parameters::kMainLikelihoodStdDevRemoved(), _likelihoodStdDevRemoved);
	Parameters::parse(parameters, Parameters::kMainLikelihoodNullValuesIgnored(), _likelihoodNullValuesIgnored);
	Parameters::parse(parameters, Parameters::kMainNeighborhoodActionsSelection(), _neighborhoodActionsSelection);

	if((iter=parameters.find(Parameters::kMainWorkingDirectory())) != parameters.end())
	{
		this->setWorkingDirectory(iter->second.c_str());
	}

	// By default, we create our strategies if they are not already created.
	// If they already exists, we check the parameters if a change is requested

	if(!_memory)
	{
		_memory = new Memory(parameters);
		_memory->init(DB_TYPE, _wDir + kDefaultDatabaseName, false, parameters);
	}
	else
	{
		_memory->parseParameters(parameters);
	}

	// Bayes filter, create one if not exists
	if(!_bayesFilter)
	{
		_bayesFilter = new BayesFilter(parameters);
	}
	else
	{
		_bayesFilter->parseParameters(parameters);
	}
}

int Semol::getLoopClosureId() const
{
	return _lcHypothesisId;
}

int Semol::getReactivatedId() const
{
	return _reactivateId;
}

int Semol::getLastSignatureId() const
{
	int id = 0;
	if(_memory && _memory->getLastNode())
	{
		id = _memory->getLastNode()->id();
	}
	return id;
}

std::list<int> Semol::getWorkingMem() const
{
	ULOGGER_DEBUG("");
	std::list<int> mem;
	if(_memory)
	{
		mem = std::list<int>(_memory->getWorkingMem().begin(), _memory->getWorkingMem().end());
		mem.remove(-1);// Ignore the virtual signature (if here)
	}
	return mem;
}

std::map<int, int> Semol::getWeights() const
{
	ULOGGER_DEBUG("");
	std::map<int, int> weights;
	if(_memory)
	{
		weights = _memory->getWeights();
		weights.erase(-1);// Ignore the virtual signature (if here)
	}
	return weights;
}

std::set<int> Semol::getStMem() const
{
	ULOGGER_DEBUG("");
	std::set<int> mem;
	if(_memory)
	{
		mem = _memory->getStMem();
	}
	return mem;
}

int Semol::getTotalMemSize() const
{
	ULOGGER_DEBUG("");
	int memSize = 0;
	if(_memory)
	{
		const Node * s  =_memory->getLastNode();
		if(s)
		{
			memSize = s->id();
		}
	}
	return memSize;
}

void Semol::resetMemory(bool dbOverwritten)
{
	if(_memory)
	{
		_memory->init(DB_TYPE, _wDir + kDefaultDatabaseName, dbOverwritten);
		if(_bayesFilter)
		{
			_bayesFilter->reset();
		}
	}
	else if(dbOverwritten)
	{
		// FIXME May be not work with other database type.
		// May be memory should be already created here, and use init above...
		UFile::erase(_wDir + kDefaultDatabaseName);
	}
	_reactivateId = 0;
	_actuatorsLink = NeighborLink(0);
	_lastLcHypothesisValue = 0;
}

bool Semol::process(const std::list<SensorRaw> & sensors, const std::list<Actuator> & actuators)
{
	UDEBUG("");

	//============================================================
	// Initialization
	//============================================================
	UTimer timer;
	UTimer timerTotal;
	double timeRetrieval = 0;
	double timeAddLoopClosureLink = 0;
	double timeRetrievalDbAccess = 0;
	double timeLikelihoodCalculation = 0;
	double timePosteriorCalculation = 0;
	double timeHypothesesCreation = 0;
	double timeActionSelection = 0;
	double timeTransfer = 0;
	double timeMemoryCleanup = 0;
	double timeEmptyingTrash = 0;
	double timeJoiningTrash = 0;
	double timeStatsCreation = 0;
	std::map<std::string, float> memUpdateStats;

	int refId = Memory::kIdInvalid;
	float hypothesisRatio = 0.0f; // Only used for statistics
	float highestHypothesisSim = 0.0f; // Only used for statistics
	bool rejectedHypothesis = false;

	std::map<int, std::vector<float> > rawLikelihoods;
	std::map<int, std::vector<float> > adjustedLikelihoods;
	std::map<int, float> likelihood;
	std::map<int, int> weights;
	std::map<int, float> posterior;
	std::pair<int, float> hypothesis(0, 0.0f);
	std::list<std::pair<int, float> > reactivateHypotheses;

	std::map<int, int> childCount;
	std::set<int> signaturesRetrieved;

	const Node * currentNode = 0;
	std::vector<int> currentNodeSensors;
	const Node * loopNode = 0;

	_lcHypothesisId = 0;
	int neighborSelected = _reactivateId;
	int actionsChosen = 0; // for stats

	std::set<int> immunizedLocations;

	statistics_ = Statistics(); // reset statistics...

	//============================================================
	// Wait for an image...
	//============================================================
	if(!sensors.size())
	{
		ULOGGER_INFO("sensors list is null...");
		return false;
	}

	timer.start();
	timerTotal.start();

	if(!_memory || !_bayesFilter)
	{
		UFATAL("SeMoLearning is not initialized, data received is ignored.");
	}

	//============================================================
	// Memory Update : Location creation + Rehearsal
	//============================================================
	ULOGGER_INFO("Updating memory...");
	if(!_memory->update(sensors, actuators, statistics_))
	{
		return false;
	}
	currentNode = _memory->getLastNode();
	currentNodeSensors = _memory->getLastNodeSensors();
	if(!currentNode)
	{
		UFATAL("Not supposed to be here...last signature is null?!?");
	}
	ULOGGER_INFO("Processing node %d", currentNode->id());
	refId = currentNode->id();
	timer.restart();

	//============================================================
	// Bayes filter update
	//============================================================
	if(!currentNode->isEmpty())
	{
		// If the working memory is empty, don't do the detection. It happens when it
		// is the first time the detector is started (there needs some images to
		// fill the short-time memory before a signature is added to the working memory).
		if(_memory->getWorkingMemSize())
		{
			//============================================================
			// Likelihood computation
			// Get the likelihood of the new signature
			// with all images contained in the working memory + reactivated.
			//============================================================
			ULOGGER_INFO("computing likelihood...");
			const std::set<int> & wm = _memory->getWorkingMem();
			std::list<int> signaturesToCompare(wm.begin(), wm.end());
			rawLikelihoods = _memory->computeLikelihoods(currentNodeSensors, signaturesToCompare);

			// Adjust the likelihood
			adjustedLikelihoods = rawLikelihoods;
			this->adjustLikelihood(adjustedLikelihoods);

			// Apply sensory attention
			likelihood = _memory->applySensoryAttention(adjustedLikelihoods, true);

			timeLikelihoodCalculation = timer.ticks();
			ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);

			//============================================================
			// Apply the Bayes filter
			//  Posterior = Likelihood x Prior
			//============================================================
			ULOGGER_INFO("getting posterior...");

			// Compute the posterior
			posterior = _bayesFilter->update(_memory, likelihood);
			timePosteriorCalculation = timer.ticks();
			ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);

			//for(std::map<int,float>::iterator iter=posterior.begin(); iter!=posterior.end(); ++iter)
			//{
			//	UDEBUG("posterior (%d) = %f", iter->first, iter->second);
			//}
			timer.start();
			//============================================================
			// Select the highest hypothesis
			//============================================================
			ULOGGER_INFO("creating hypotheses...");
			if(posterior.size())
			{
				if(posterior.size())
				{
					hypothesis = this->selectHypothesis(posterior,
							likelihood,
							_selectionNeighborhoodSummationUsed,
							_selectionLikelihoodUsed);
					// When using a virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
					if(posterior.begin()->first == Memory::kIdVirtual)
					{
						hypothesis.second = 1-posterior.begin()->second;
					}
				}
			}
			timeHypothesesCreation = timer.ticks();
			ULOGGER_INFO("Hypothesis=%d, value=%f, timeHypothesesCreation=%fs", hypothesis.first, hypothesis.second, timeHypothesesCreation);

			if(hypothesis.first > 0)
			{
				std::map<int, std::vector<float> > highestLikelihood;
				highestLikelihood.insert(std::make_pair(hypothesis.first, rawLikelihoods.at(hypothesis.first)));
				std::map<int, float> highestLikelihoodMerged = _memory->applySensoryAttention(highestLikelihood, false);
				highestHypothesisSim = highestLikelihoodMerged.begin()->second;
				UDEBUG("highestHypothesisSim=%f _loopSimilarity=%f", highestHypothesisSim, _loopSimilarity);

				// Loop closure Threshold
				// When _loopThr=0, accept loop closure if the hypothesis is over
				// the virtual (new) place hypothesis.
				if((_loopThr > 0 && hypothesis.second >= _loopThr) ||
				   (_loopThr == 0 && posterior.begin()->first == Memory::kIdVirtual && hypothesis.second > posterior.begin()->second))
				{
					//============================================================
					// Hypothesis verification for loop closure with geometric
					// information (like the epipolar geometry or using the local map
					// associated with the signature)
					//============================================================
					if(hypothesis.second >= _loopRatio*_lastLcHypothesisValue && _lastLcHypothesisValue)
					{
						_lcHypothesisId = hypothesis.first;
					}
					else
					{
						rejectedHypothesis = true;
					}
				}
				else if(hypothesis.second < _loopRatio*_lastLcHypothesisValue)
				{
					// Used for Precision-Recall computation.
					// When analysing logs, it's convenient to know
					// if the hypothesis would be rejected if T_loop would be lower.
					rejectedHypothesis = true;
				}

				//============================================================
				// Retrieval id update
				//============================================================
				_reactivateId = hypothesis.first;

				//for statistic...
				hypothesisRatio = _lastLcHypothesisValue>0?hypothesis.second/_lastLcHypothesisValue:0;
				_lastLcHypothesisValue = hypothesis.second;

			}
		} // if(_memory->getWorkingMemSize())
	}// !isBadSignature

	//============================================================
	// Before retrieval, make sure the trash has finished
	//============================================================
	_memory->joinTrashThread();
	timeEmptyingTrash = _memory->getDbSavingTime();
	timeJoiningTrash = timer.ticks();
	ULOGGER_INFO("Time emptying memory trash = %fs,  joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);

	// also copy weights before the memory is changed
	if(_publishStats && (_publishLikelihood || _publishPdf))
	{
		weights = _memory->getWeights();
	}

	//============================================================
	// RETRIEVAL : Loop closure neighbors reactivation
	//============================================================
	if(_reactivateId > 0 )
	{
		//Load neighbors
		ULOGGER_INFO("Retrieving locations... around id=%d", _reactivateId);
		unsigned int margin = _bayesFilter->getPredictionLC().size()-1;//_memory->getMaxStMemSize();

		UTimer timeGetN;
		unsigned int nbLoadedFromDb = 0;
		std::set<int> reactivatedIdsSet;
		std::list<int> reactivatedIds;
		double timeGetNeighborsTimeDb = 0.0;
		double timeGetNeighborsSpaceDb = 0.0;
		std::map<int, int> neighbors;
		bool firstPassDone = false;
		unsigned int m = 0;
		int nbDirectNeighborsInDb = 0;

		// neighbors SPACE, already added direct neighbors will be ignored
		ULOGGER_DEBUG("In SPACE");
		neighbors = _memory->getNeighborsId(_reactivateId,
				margin,
				_maxRetrieved,
				false,
				true,
				false,
				&timeGetNeighborsSpaceDb);
		UDEBUG("%d neighbors of reactivateId=%d", neighbors.size(), _reactivateId);
		m = 0;
		firstPassDone = false;
		while(m < margin)
		{
			std::set<int> idsSorted;
			for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
			{
				if(!firstPassDone && _memory->isInSTM(iter->first))
				{
					neighbors.erase(iter++);
				}
				else if((unsigned int)iter->second == m)
				{
					if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
					{
						idsSorted.insert(iter->first);
						reactivatedIdsSet.insert(iter->first);

						if(m == 1 && _memory->getNode(iter->first) == 0)
						{
							++nbDirectNeighborsInDb;
						}

						// immunize neighbors in space when loop closures are merged
						if(m<margin)
						{
							const Node * s = _memory->getNode(iter->first);
							bool immunize = false;
							if(s && s->getLoopClosureIds().size())
							{
								for(std::set<int>::const_iterator jter=s->getLoopClosureIds().begin(); jter!=s->getLoopClosureIds().end(); ++jter)
								{
									if(_memory->isInSTM(*jter))
									{
										immunize = true;
										break;
									}
								}
							}
							else
							{
								immunize = true; // retrieved signatures are immunized
							}
							if(immunize)
							{
								//immunized locations in the neighborhood from being transferred
								immunizedLocations.insert(iter->first);
							}
							//UDEBUG("ns=%d m=%d immunized=%d", iter->first, iter->second, immunize?1:0);
						}
						else
						{
							UDEBUG("ns=%d m=%d immunized=0", iter->first, iter->second);
						}
					}
					std::map<int, int>::iterator tmp = iter++;
					neighbors.erase(tmp);
				}
				else
				{
					++iter;
				}
			}
			firstPassDone = true;
			reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
			++m;
		}
		ULOGGER_INFO("margin=%d, "
				"reactivatedIds.size=%d, "
				"nbLoadedFromDb=%d, "
				"nbDirectNeighborsInDb=%d, "
				"time=%fs (%fs %fs)",
				margin,
				reactivatedIds.size(),
				(int)nbLoadedFromDb,
				nbDirectNeighborsInDb,
				timeGetN.ticks(),
				timeGetNeighborsTimeDb,
				timeGetNeighborsSpaceDb);


		// For merging issue, loop closure hypothesis must have all its neighbors loaded
		signaturesRetrieved = _memory->reactivateNodes(
				reactivatedIds,
				nbDirectNeighborsInDb>(int)_maxRetrieved?nbDirectNeighborsInDb:_maxRetrieved,
				timeRetrievalDbAccess);

		ULOGGER_INFO("retrieval of %d reactivatedIds=%fs (db time = %fs)", (int)signaturesRetrieved.size(), timeGetN.ticks(), timeRetrievalDbAccess);

		timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
		UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);

		// Immunize just retrieved signatures
		immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
	}
	timeRetrieval = timer.ticks();
	ULOGGER_INFO("timeRetrieval=%fs", timeRetrieval);

	//=============================================================
	// Update loop closure links
	// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
	//=============================================================
	// Make the new one the parent of the old one
	if(_lcHypothesisId>0)
	{
		rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, currentNode->id());

		if(rejectedHypothesis)
		{
			_lcHypothesisId = 0;
		}
	}
	timeAddLoopClosureLink = timer.ticks();
	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);

	//============================================================
	// Select next actions
	//============================================================
	timer.start();
	if(hypothesis.first > 0)
	{
		loopNode = _memory->getNode(hypothesis.first);
	}
	int lastActuatorId = _actuatorsLink.actuatorId();
	_actuatorsLink = NeighborLink(0);

	// only send actions if rejectLoopReason!=3 (decreasing hypotheses)
	if(loopNode && actuators.size() && (_actionsSentRejectHyp || !rejectedHypothesis) && (_lastLcHypothesisValue >= _confidenceThr))
	{
		UTimer actionTimer;
		UINFO("Selecting actions... (last aid was %d)", lastActuatorId);
		std::map<int, int> ids;
		std::map<int, const NeighborLink *> neighborsMap;
		std::map<int, const NeighborLink *> lastActuatorNeighborsMap;
		int minSTMid = 0;
		if(_memory->getStMem().size())
		{
			minSTMid = *_memory->getStMem().begin();
		}
		ids = _memory->getNeighborsId(loopNode->id(), _neighborhoodActionsSelection+1, 0, false, true, false);
		UINFO("Selecting actions... get neighbors(%d) time=%fs", ids.size(), actionTimer.ticks());
		for(std::map<int, int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
		{
			std::list<const NeighborLink *> links = _memory->getNeighborLinks(iter->first, true, true);
			for(std::list<const NeighborLink *>::iterator jter = links.begin(); jter!=links.end(); ++jter)
			{
				if((*jter)->actuatorId() < minSTMid && (*jter)->actuatorId()>0)
				{
					if( lastActuatorId > 0 &&
						(*jter)->actuatorId() >= lastActuatorId - _memory->getMaxStMemSize() &&
						(*jter)->actuatorId() <= lastActuatorId + _memory->getMaxStMemSize())
					{
						lastActuatorNeighborsMap.insert(std::make_pair((*jter)->actuatorId(), *jter));
					}
					else
					{
						neighborsMap.insert(std::make_pair((*jter)->actuatorId(), *jter));
					}
				}
			}
			UDEBUG("get neighbor links(%d) neighbor %d margin=%d", links.size(), iter->first, iter->second);
		}

		std::list<const NeighborLink *> neighbors;
		// ids around lastActuatorId then others
		for(std::map<int, const NeighborLink *>::iterator iter=neighborsMap.begin(); iter!=neighborsMap.end(); ++iter)
		{
			neighbors.push_front(iter->second);
		}
		for(std::map<int, const NeighborLink *>::iterator iter=lastActuatorNeighborsMap.begin(); iter!=lastActuatorNeighborsMap.end(); ++iter)
		{
			neighbors.push_front(iter->second);
		}
		UINFO("Selecting actions... get neighbor links time=%fs", actionTimer.ticks());

		UINFO("Comparison actions...");
		float currentMaxSim = -1;
		int maxActionsChecked = 9999;
		UINFO("Actions: neighbors.size=%d (n=%d, lastn=%d), to choose %d", neighbors.size(), neighborsMap.size(), lastActuatorNeighborsMap.size(), maxActionsChecked);

		std::map<float, int> simResults; // <sim, aid>

		const NeighborLink * chosenActuators = 0;
		for(std::list<const NeighborLink *>::const_iterator iter=neighbors.begin();
			iter!=neighbors.end() && currentMaxSim!=1.0f && actionsChosen < maxActionsChecked;
			++iter)
		{
			UDEBUG("iter->historyIds()=%d _memory->getLastHistoryIds()=%d", (*iter)->sensorHistoryIds().size(), _memory->getHistoryIds().size());
			std::vector<float> similarities = _memory->compareHistory((*iter)->sensorHistoryIds(), _memory->getHistoryIds());
			float sim = _memory->applySensoryAttention(similarities);
			UDEBUG("Neighbor baseIds comparison with %d (aid=%d) = %f", (*iter)->toId(), (*iter)->actuatorId(), sim);
			if(sim > currentMaxSim)
			{
				currentMaxSim = sim;
				chosenActuators = *iter;
			}
			++actionsChosen;
			simResults.insert(std::make_pair(sim, (*iter)->actuatorId()));
		}
		if(chosenActuators)
		{
			_actuatorsLink = *chosenActuators;
		}
		UDEBUG("Action (aid=%d) taken from neighbor %d", _actuatorsLink.actuatorId(), _actuatorsLink.toId());
		UINFO("Comparison actions... time=%fs", actionTimer.ticks());
		for(std::map<float, int>::iterator iter = simResults.begin(); iter!=simResults.end(); ++iter)
		{
			UDEBUG("aid=%d sim=%f", iter->first, iter->second);
		}
	}

	timeActionSelection = timer.ticks();
	ULOGGER_INFO("timeActionSelection=%fs",timeActionSelection);


	//============================================================
	// Prepare statistics
	//============================================================
	// Data used for the statistics event and for the log files
	int lcHypothesisReactivated = 0;
	if(loopNode)
	{
		lcHypothesisReactivated = loopNode->isSaved()?1.0f:0.0f;
	}

	float vpLikelihood = 0.0f;
	if(likelihood.size() && likelihood.begin()->first == -1)
	{
		vpLikelihood = likelihood.begin()->second;
	}
	float vpHypothesis = 0.0f;
	if(posterior.find(Memory::kIdVirtual) != posterior.end())
	{
		vpHypothesis = posterior.at(Memory::kIdVirtual);
	}

	// only prepare statistics if required or when there is a loop closure
	if(_lcHypothesisId || _actuatorsLink.actuators().size() || _publishStats)
	{
		ULOGGER_INFO("set stats...");
		statistics_.setRefImageId(refId);
		if(_lcHypothesisId != Memory::kIdInvalid)
		{
			statistics_.setLoopClosureId(_lcHypothesisId);
			ULOGGER_INFO("Loop closure detected! With id=%d", _lcHypothesisId);
		}
		if(_actuatorsLink.actuators().size())
		{
			statistics_.setActuators(_actuatorsLink.actuators());
		}
		if(_publishStats && refId != Memory::kIdInvalid)
		{
			ULOGGER_INFO("send all stats...");
			statistics_.setExtended(1);

			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), hypothesis.first);
			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), hypothesis.second);
			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_sim(), highestHypothesisSim);
			statistics_.addStatistic(Statistics::kLoopHypothesis_retrieved(), lcHypothesisReactivated);
			statistics_.addStatistic(Statistics::kLoopVp_likelihood(), vpLikelihood);
			statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
			statistics_.addStatistic(Statistics::kLoopReactivateId(), _reactivateId);
			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
			statistics_.addStatistic(Statistics::kLoopActions_size(), (int)_actuatorsLink.actuators().size());
			statistics_.addStatistic(Statistics::kLoopActions_id(), _actuatorsLink.actuatorId());
			statistics_.addStatistic(Statistics::kLoopActions_of(), neighborSelected);
			statistics_.addStatistic(Statistics::kLoopActions_chosen(), actionsChosen);

			statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMemSize());
			statistics_.addStatistic(Statistics::kMemoryShort_term_memory_size(), _memory->getStMemSize());
			statistics_.addStatistic(Statistics::kMemoryNodes_retrieved(), (float)signaturesRetrieved.size());

			// timing...
			statistics_.addStatistic(Statistics::kTimingRetrieval(), timeRetrieval*1000);
			statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
			statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
			statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
			statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
			statistics_.addStatistic(Statistics::kTimingAction_selection(), timeActionSelection*1000);

			// memory update timings
			for(std::map<std::string, float>::iterator iter = memUpdateStats.begin(); iter!=memUpdateStats.end(); ++iter)
			{
				statistics_.addStatistic(iter->first, iter->second);
			}

			//Epipolar geometry constraint
			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);

			// Sensory attention
			const std::vector<float> & sensoryWeights = _memory->getSensoryAttentionWeights();
			const std::vector<float> & sensorySimilarities = _memory->getSensoryAttentionSimilarities();
			if(sensoryWeights.size() && sensorySimilarities.size() == sensoryWeights.size())
			{
				for(unsigned int i=0; i<sensoryWeights.size(); ++i)
				{
					statistics_.addStatistic(uFormat("Attention/w%d/", i), sensoryWeights[i]);
					statistics_.addStatistic(uFormat("Attention/s%d/", i), sensorySimilarities[i]);
				}
			}

			UDEBUG("");
			if(_publishRawData)
			{
				statistics_.setRefRawData(sensors); // raw data
				if(loopNode)
				{
					lcHypothesisReactivated = 0;
					if(loopNode->isSaved())
					{
						lcHypothesisReactivated = 1;
					}

					UTimer tmpTimer;
					std::list<SensorRaw> data = _memory->getRawData(loopNode->id());

					if(tmpTimer.elapsed() > 0.03)
					{
						UWARN("getting raw data time = %fs", tmpTimer.ticks());
					}
					statistics_.setLoopClosureRawData(data);
				}
			}
			UDEBUG("");
			if(_publishLikelihood || _publishPdf)
			{
				// Child count by parent signature on the root of the memory ... for statistics
				statistics_.setWeights(weights);
				if(_publishPdf)
				{
					statistics_.setPosterior(posterior);
				}
				if(_publishLikelihood)
				{
					statistics_.setLikelihood(likelihood);
					statistics_.setLikelihoods(adjustedLikelihoods);
					statistics_.setRawLikelihoods(rawLikelihoods);
				}
			}
			UDEBUG("");
			if(_publishSensorData)
			{
				if(currentNode)
				{
					std::list<std::vector<float> > data;
					for(std::vector<int>::const_iterator iter = currentNode->getSensors().begin();
						iter!=currentNode->getSensors().end();
						++iter)
					{
						if(*iter)
						{
							const Sensor * s = _memory->getSensor(*iter);
							if(s)
							{
								data.push_back(s->descriptor());
							}
							else
							{
								UERROR("sensor %d not found in memory ?!?", *iter);
								data.push_back(std::vector<float>());
							}
						}
						else
						{
							data.push_back(std::vector<float>());
						}
					}
					statistics_.setRefSensorData(data);
				}
				if(loopNode)
				{
					std::list<std::vector<float> > data;
					for(std::vector<int>::const_iterator iter = loopNode->getSensors().begin();
						iter!=loopNode->getSensors().end();
						++iter)
					{
						if(*iter)
						{
							const Sensor * s = _memory->getSensor(*iter);
							if(s)
							{
								data.push_back(s->descriptor());
							}
							else
							{
								UERROR("sensor %d not found in memory ?!?", *iter);
								data.push_back(std::vector<float>());
							}
						}
						else
						{
							data.push_back(std::vector<float>());
						}
					}
					statistics_.setLoopSensorData(data);
				}
			}
			UDEBUG("");
		}

		timeStatsCreation = timer.ticks();
		ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
	}

	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
	//This will also remove rehearsed signatures
	int nodesTransferred = 0;
	nodesTransferred += _memory->cleanup();
	timeMemoryCleanup = timer.ticks();
	ULOGGER_INFO("timeMemoryCleanup = %fs...", timeMemoryCleanup);

	//============================================================
	// TRANSFER
	//============================================================
	// If time allowed for the detection exceeds the limit of
	// real-time, move the oldest signature with less frequency
	// entry (from X oldest) from the short term memory to the
	// long term memory.
	//============================================================
	double totalTime = timerTotal.ticks();
	ULOGGER_INFO("Total time processing = %fs...", totalTime);
	timer.start();
	if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
		(_maxMemoryAllowed != 0 && _memory->getWorkingMemSize() > _maxMemoryAllowed))
	{
		ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMemSize(), _maxMemoryAllowed);
		nodesTransferred = _memory->forget(immunizedLocations);
	}

	timeTransfer = timer.ticks();
	ULOGGER_INFO("Time limit reached processing = %f...", timeTransfer);

	//Start trashing
	_memory->startTrashThread();


	//==============================================================
	// Finalize statistics and log files
	//==============================================================

	if(_publishStats)
	{
		statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
		statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000);
		statistics_.addStatistic(Statistics::kTimingForgetting(), timeTransfer*1000);
		statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
		statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
		statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
		statistics_.addStatistic(Statistics::kMemoryNodes_transferred(), nodesTransferred);
	}

	return true;
}

// SETTERS
void Semol::setMaxTimeAllowed(float maxTimeAllowed)
{
	//must be positive, 0 mean inf time allowed (no time limit)
	_maxTimeAllowed = maxTimeAllowed;
	if(_maxTimeAllowed < 0)
	{
		ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf).");
		_maxTimeAllowed = 0;
	}
	else if(_maxTimeAllowed < 1)
	{
		ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
	}
}

void Semol::setDataBufferSize(int size)
{
	if(size < 0)
	{
		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
		_sensorsBufferMaxSize = 0;
	}
	else
	{
		_sensorsBufferMaxSize = size;
	}
}

void Semol::setWorkingDirectory(std::string path)
{
	if(path.size() && (path.at(path.size()-1) != '\\' || path.at(path.size()-1) != '/' ))
	{
		path += UDirectory::separator();
	}

	if(!path.empty() && UDirectory::exists(path))
	{
		ULOGGER_DEBUG("Comparing new path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
		if(path.compare(_wDir) != 0)
		{
			_wDir = path;
			if(_memory)
			{
				_memory->init(DB_TYPE, _wDir + kDefaultDatabaseName);
			}
		}
	}
	else
	{
		ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
	}
}

bool Semol::process(const SensorRaw & data)
{
	std::list<SensorRaw> sensors;
	sensors.push_back(data);
	return this->process(sensors);
}

void Semol::dumpData() const
{
	if(_memory)
	{
		_memory->dumpMemory(this->getWorkingDir());
	}
}


void Semol::adjustLikelihood(std::map<int, std::vector<float> > & likelihood) const
{
	ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
	UTimer timer;
	timer.start();
	if(likelihood.size()==0)
	{
		return;
	}

	std::vector<std::vector<float> > allValues = uValues(likelihood);
	UDEBUG("allValues.size=%d", allValues.size());
	UDEBUG("allValues[0].size=%d", allValues[0].size());

	// Use only non-null values
	std::vector<std::list<float> > values(allValues[0].size());
	for(unsigned int i=0; i<allValues.size(); ++i)
	{
		for(unsigned int j=0; j<allValues[i].size(); ++j)
		{
			if((allValues[i].at(j) >= 0 && !_likelihoodNullValuesIgnored) ||
			   (allValues[i].at(j) > 0 && _likelihoodNullValuesIgnored))
			{
				values[j].push_back(allValues[i].at(j));
			}
		}
	}
	UDEBUG("values.size=%d", values.size());

	if(likelihood.begin()->first == -1)
	{
		for(unsigned int i=0; i<values.size(); ++i)
		{
			//if(likelihood.begin()->second.at(i) != 0)
			//{
				//likelihood.begin()->second.at(i) = 0;
				values[i].pop_front();
			//}
		}
	}

	std::vector<float> mean(values.size());
	std::vector<float> stdDev(values.size());
	for(unsigned int i=0; i<values.size(); ++i)
	{
		//Compute mean
		mean[i] = uMean(values[i]);

		//Compute std dev
		stdDev[i] = uStdDev(values[i], mean[i]);
	}


	//Adjust likelihood with mean and standard deviation (see Angeli phd)
	float epsilon = 0.0001;
	std::vector<float> max(likelihood.begin()->second.size(), 0.0f);
	std::vector<float> min(likelihood.begin()->second.size(), 0.0f);
	std::vector<int> maxId(likelihood.begin()->second.size(), 0);
	for(std::map<int, std::vector<float> >::iterator iter=likelihood.begin(); iter!= likelihood.end(); ++iter)
	{
		if(iter->first > 0)
		{
			for(unsigned int i=0; i<iter->second.size(); ++i)
			{
				float value = iter->second.at(i);
				if(value > mean[i]+(!_likelihoodStdDevRemoved?0:stdDev[i]) && mean[i] /*&& (value >= 0.9 || iter->first<0)*/)
				{
					if(_likelihoodStdDevRemoved)
					{
						iter->second.at(i) = (value-mean[i]-(stdDev[i]-epsilon))/stdDev[i] + 1;
					}
					else
					{
						iter->second.at(i) = (value-mean[i])/stdDev[i] + 1;
					}
					if(value > max[i])
					{
						max[i] = value;
						maxId[i] = iter->first;
					}
				}
				else if(value == 1.0f && stdDev[i] == 0)
				{
					iter->second.at(i) = 1.0f;
					if(value > max[i])
					{
						max[i] = value;
						maxId[i] = iter->first;
					}
				}
				else
				{
					if(value > 0 && (value < min[i] || min[i] == 0.0f))
					{
						min[i] = value;
					}
					iter->second.at(i) = 1.0f;
				}
			}
		}
	}

	if(likelihood.begin()->first == -1)
	{
		for(unsigned int i=0; i<likelihood.begin()->second.size(); ++i)
		{
			/*if(stdDev[i] > epsilon && max[i])
			{
				likelihood.begin()->second.at(i) = mean[i]/stdDev[i] + 1;
			}
			else if(max[i])
			{
				likelihood.begin()->second.at(i) = 2.0f;
			}
			else
			{
				likelihood.begin()->second.at(i) = 2.0f; // infinity ...
			}*/
			//if(likelihood.begin()->second.at(i)<1.0f)
		//	{
				likelihood.begin()->second.at(i) = 1.0f;
		//	}
			//else if(likelihood.begin()->second.at(i) > 2.0f)
			//{
			//	likelihood.begin()->second.at(i) = 2.0f;
			//}
		}
	}

	double time = timer.ticks();
	for(unsigned int i=0; i<mean.size(); ++i)
	{
		ULOGGER_DEBUG("for i=%d, mean=%f, stdDev=%f, max=%f, min=%f, maxId=%d, time=%fs", i, mean[i], stdDev[i], max[i], min[i], maxId[i], time);
	}
}

std::pair<int, float> Semol::selectHypothesis(const std::map<int, float> & posterior,
							const std::map<int, float> & likelihood,
							bool neighborSumUsed,
							bool likelihoodUsed) const
{
	ULOGGER_DEBUG("");
	if(!_bayesFilter || !_memory)
	{
		ULOGGER_ERROR("SeMoLearning must be initialized first!");
		return std::pair<int, float>(0, 0.0f);
	}
	if(posterior.size() == 0)
	{
		ULOGGER_ERROR("Posterior parameter size = 0?");
		return std::pair<int, float>(0, 0.0f);
	}

	/*std::vector<float> values = uValues(posterior);
	std::list<unsigned int> maxima = uLocalMaxima(values.data()+1, values.size()-1);
	for(std::list<unsigned int>::iterator iter = maxima.begin(); iter!=maxima.end();)
	{
		if(values[*iter+1] >= 0.01)
		{
			++iter;
		}
		else
		{
			iter = maxima.erase(iter);
		}
	}
	if(maxima.size() > 1)
	{
		std::vector<int> keys = uKeys(posterior);
		for(std::list<unsigned int>::iterator iter = maxima.begin(); iter!=maxima.end(); ++iter)
		{
			UDEBUG("Local maxima at %d with value %f", keys[*iter+1], values[*iter+1]);
		}
	}*/

	int id = 0;
	float value;
	float totalLoopClosure = 0.0f;
	int hypothesisId = 0;
	float hypothesisValue = 0.0f;
	UTimer timer;
	timer.start();
	UTimer timerGlobal;
	timerGlobal.start();
	for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
	{
		value = (*iter).second;
		id = (*iter).first;
		if(id > 0)
		{
			totalLoopClosure += value;

			if(value > hypothesisValue)
			{
				hypothesisId = id;
				hypothesisValue = (*iter).second;
			}
		}
	}

	if((likelihoodUsed || neighborSumUsed) && hypothesisId > 0)
	{
		//select highest likelihood from the neighborhood of the highest hypothesis
		double dbAccessTime = 0.0;
		std::map<int, int> neighbors = _memory->getNeighborsId(hypothesisId, _bayesFilter->getPredictionLC().size()-1, 0, false, false, false, &dbAccessTime);
		float oldlikelihood = likelihood.at(hypothesisId);
		int oldId = hypothesisId;
		float highestLikelihood = oldlikelihood;
		bool modified = false;
		std::set<int> loopIds, childIds;
		UDEBUG("start highestLikelihood=%f, neighbors.size()=%d", highestLikelihood, neighbors.size());
		for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
		{
			if(likelihoodUsed)
			{
				std::map<int, float>::const_iterator sim = likelihood.find(jter->first);
				if(sim != likelihood.end())
				{
					if(sim->second > highestLikelihood)
					{
						UDEBUG("sim->first=%d, sim->second=%f", sim->first, sim->second);
						highestLikelihood = sim->second;
						hypothesisId = sim->first;
						modified = true;
					}
				}
			}
			if(neighborSumUsed)
			{
				if(jter->first != oldId)
				{
					std::map<int, float>::const_iterator tmpIter = posterior.find(jter->first);
					if(tmpIter!=posterior.end())
					{
						hypothesisValue += tmpIter->second;
					}
				}
			}
		}
		if(modified)
		{
			UDEBUG("Lc hypothesis (%d) changed to neighbor (%d) with higher likelihood (%f->%f)", oldId, hypothesisId, oldlikelihood, highestLikelihood);
		}
	}


	ULOGGER_DEBUG("time=%fs", timerGlobal.ticks());
	return std::make_pair(hypothesisId, hypothesisValue);
}

void Semol::dumpPrediction() const
{
	if(_memory && _bayesFilter)
	{
		const std::set<int> & wm = _memory->getWorkingMem();
		cv::Mat prediction = _bayesFilter->generatePredictionMatrix(_memory, wm);

		FILE* fout = 0;
		std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";
		#ifdef _MSC_VER
			fopen_s(&fout, fileName.c_str(), "w");
		#else
			fout = fopen(fileName.c_str(), "w");
		#endif

		if(fout)
		{
			for(int i=0; i<prediction.rows; ++i)
			{
				for(int j=0; j<prediction.cols; ++j)
				{
					fprintf(fout, "%f ",((float*)prediction.data)[j + i*prediction.cols]);
				}
				fprintf(fout, "\n");
			}
			fclose(fout);
		}
	}
	else
	{
		UWARN("Memory and/or the Bayes filter are not created");
	}
}


void Semol::generateGraph(const std::string & path, int id, int margin)
{
	if(_memory)
	{
		_memory->joinTrashThread(); // make sure the trash is flushed

		if(id>0)
		{
			std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false, false);

			if(ids.size() > 0)
			{
				ids.insert(std::pair<int,int>(id, 0));
				std::set<int> idsSet;
				for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
				{
					idsSet.insert(idsSet.end(), iter->first);
				}
				_memory->generateGraph(path, idsSet);
			}
			else
			{
				UERROR("No neighbors found for node %d.", id);
			}
		}
		else
		{
			_memory->generateGraph(path);
		}
	}
}

void Semol::readParameters(const std::string & configFile, ParametersMap & parameters)
{
	CSimpleIniA ini;
	ini.LoadFile(configFile.c_str());
	const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core");
	if(keyValMap)
	{
		for(CSimpleIniA::TKeyVal::const_iterator iter=keyValMap->begin(); iter!=keyValMap->end(); ++iter)
		{
			std::string key = (*iter).first.pItem;
			key = uReplaceChar(key, '\\', '/'); // Ini files use \ by default for separators, so replace them
			ParametersMap::iterator jter = parameters.find(key);
			if(jter != parameters.end())
			{
				parameters.erase(jter);
			}
			parameters.insert(ParametersPair(key, (*iter).second));
		}
	}
	else
	{
		ULOGGER_WARN("Section \"Core\" in %s doesn't exist... "
				    "Ignore this warning if the ini file does not exist yet. "
				    "The ini file will be automatically created when this node will close.", configFile.c_str());
	}
}

void Semol::writeParameters(const std::string & configFile, const ParametersMap & parameters)
{
	CSimpleIniA ini;
	ini.LoadFile(configFile.c_str());

	for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
	{
		std::string key = (*i).first;
		key = uReplaceChar(key, '/', '\\'); // Ini files use \ by default for separators, so replace the /
		ini.SetValue("Core", key.c_str(), (*i).second.c_str(), NULL, true);
	}

	ini.SaveFile(configFile.c_str());
}

} // namespace rtabmap
